package com.arithmetic;

import android.util.Log;

import com.DataProcess.Robot;

/**
 * Created by tjpu on 2016/10/13.
 */
public class SaveOut {

    static String thisLogTag = "SaveOut";
    static double DANGEROUS_OUT = 250;

    public static double[] getPWM(double[] input) {
        double[] pwm = new double[2];
        System.arraycopy(input,0, pwm,0,2);
        if (pwm[0] > DANGEROUS_OUT || pwm[1] > DANGEROUS_OUT) {
            pwm[0] = 0;
            pwm[1] = 0;
            Robot.isEnable = false;
            Log.e(thisLogTag,"out too large make disEnable");
        }

        //Todo:if lost CanData

        if (!Robot.isEnable) {
            pwm[0] = 0;
            pwm[1] = 0;
            MotorsControl.left.cleanPID();
            MotorsControl.right.cleanPID();
        }

        pwm[0] = Math.rint(pwm[0]);
        pwm[1] = Math.rint(pwm[1]);

        return pwm;
    }
}
